#include "discrete_circle_system.h"

// for optimization debug
//#define KNITRO	1
#define WITH_ANGLE	1

namespace Particle_packing
{
	Distance_field_grid_2::Distance_field_grid_2(const Container_box& container, size_t nb_rows, size_t nb_cols)
		: grid(container.x_min(), container.x_max(), container.y_min(), container.y_max(), nb_rows, nb_cols)
	{
		for (size_t r = 0; r <= grid.nb_rows(); r++)
		{
			for (size_t c = 0; c <= grid.nb_columns(); c++)
			{
				grid.at(r, c).x = grid.xmin() + c*grid.xstep();
				grid.at(r, c).y = grid.ymin() + r*grid.ystep();
			}
		}
	}
	Distance_field_grid_2::~Distance_field_grid_2()
	{
		clear();
	}
	void Distance_field_grid_2::clear()
	{
		grid.for_each_node(std::mem_fun_ref(&DistanceNode::clear));
		for (size_t i = 0; i < local_fields.size(); i++)
			delete local_fields[i];
		local_fields.clear();
	}
	void Distance_field_grid_2::append_distance_field(const Particle_circle& circle)
	{
		for (size_t r = 0; r <= grid.nb_rows(); r++)
		{
			for (size_t c = 0; c <= grid.nb_columns(); c++)
			{
				//Point_2 p(grid.xmin()+c*grid.xstep(), grid.ymin()+r*grid.ystep());
				Point_2 p(grid.at(r, c).x, grid.at(r, c).y);
				double tmp = CGAL::sqrt(CGAL::squared_distance(circle.center(), p));
				double d = tmp - circle.radius();
				grid.at(r, c).dist_polygon.push_back(d);
				Vector_2 v(circle.center(), p);
				if (tmp == 0.0)
					grid.at(r, c).gradients.push_back(Vector_2(0,0));
				else
					grid.at(r, c).gradients.push_back(2*(1-circle.radius()/tmp)*v);
			}
		}
		// construct local frames
		Point_2 c = circle.center();
		Local_frame_2 lf(Vector_2(1.0, 0.0), Vector_2(0.0, 1.0), c);
		local_frames.push_back(lf);

		// construct local coordinates
		double grid_width = grid.xmax() - grid.xmin(), grid_height = grid.ymax() - grid.ymin();
		const int times = 3;
		local_fields.push_back(new Grid_2<LocalFieldNode>(-times*grid_width/2, times*grid_width/2, -times*grid_height/2, times*grid_height/2, grid.nb_rows()*times, grid.nb_columns()*times));
		Grid_2<LocalFieldNode>& local_grid = *local_fields.back();
		Particle_circle local_circle(Point_2(0,0), circle.squared_radius());

#ifdef _CILK_
#pragma message("Using Intel Cilk plus")
		cilk_for (size_t r = 0; r <= local_grid.nb_rows(); r++)
#else
		for (size_t r = 0; r <= local_grid.nb_rows(); r++)
#endif
		{
			for (size_t c = 0; c <= local_grid.nb_columns(); c++)
			{
				Point_2 p(local_grid.xmin()+c*local_grid.xstep(), local_grid.ymin()+r*local_grid.ystep());
				double tmp = CGAL::sqrt(CGAL::squared_distance(p, local_circle.center()));
				double d = tmp - local_circle.radius();
				if (tmp == 0)
					local_grid.at(r, c).grad = Vector_2(0,0);
				else
					local_grid.at(r, c).grad = 2*(1-local_circle.radius()/tmp)*Vector_2(local_circle.center(), p);
				assert(!Geex::Numeric::is_nan(local_grid.at(r, c).grad.x()));
				assert(!Geex::Numeric::is_nan(local_grid.at(r, c).grad.y()));
				local_grid.at(r, c).dist = d;
			}
		}
	}

	double Distance_field_grid_2::local_query(const Point_2& p, size_t particle_id, Vector_2& grad) const
	{
		const Grid_2<LocalFieldNode>& g = *local_fields[particle_id];
		if (p.x() <= g.xmin() || p.y() <= g.ymin() || p.x() >= g.xmax() || p.y() >= g.ymax() )
		{
			double w = g.xmax() - g.xmin();
			double h = g.ymax() - g.ymin();
			// TODO: it should not be arbitrarily zero vector
			grad = Vector_2(0.0, 0.0);
			//closest_point = Point_2(0.0, 0.0); //set as the centroid of the polygon
			return std::sqrt(w*w + h*h);
		}
		size_t c = std::floor((p.x() - g.xmin())/g.xstep());
		size_t r = std::floor((p.y() - g.ymin())/g.ystep());
		double frac_c = p.x() - (g.xmin() + c*g.xstep());
		double frac_r = p.y() - (g.ymin() + r*g.ystep());
		double w[4] = {frac_c*frac_r, (g.xstep()-frac_c)*(g.ystep()-frac_r), frac_c*(g.ystep()-frac_r), (g.xstep()-frac_c)*frac_r};
		const LocalFieldNode *n[4] = {&g.at(r+1, c+1), &g.at(r, c), &g.at(r, c+1), &g.at(r+1, c)};
		double cell_area = g.xstep()*g.ystep();
		double d = w[0]*n[0]->dist + w[1]*n[1]->dist + w[2]*n[2]->dist + w[3]*n[3]->dist;
		d /= cell_area;
		grad = w[0]*n[0]->grad + w[1]*n[1]->grad + w[2]*n[2]->grad + w[3]*n[3]->grad;
		grad = grad / cell_area;
		return d;
	}

	double Distance_field_grid_2::distance_query(const Point_2& p, size_t particle_id) const
	{
		if (p.x() <= grid.xmin() || p.y() <= grid.ymin() || p.x() >= grid.xmax() || p.y() >= grid.ymax() )
		{
			Vector_2 v;
			double d = local_query(local_frames[particle_id].local_coord(p), particle_id, v);
			return d;
		}
		size_t c = std::floor((p.x() - grid.xmin())/grid.xstep());
		size_t r = std::floor((p.y() - grid.ymin())/grid.ystep());
		double frac_c = p.x() - (grid.xmin() + c*grid.xstep());
		double frac_r = p.y() - (grid.ymin() + r*grid.ystep());
		double w[4] = {frac_c*frac_r, (grid.xstep()-frac_c)*(grid.ystep()-frac_r), frac_c*(grid.ystep()-frac_r), (grid.xstep()-frac_c)*frac_r};
		double d = w[0]*grid.at(r+1, c+1).dist_polygon[particle_id] + w[1]*grid.at(r, c).dist_polygon[particle_id];
		d += w[2]*grid.at(r, c+1).dist_polygon[particle_id]+w[3]*grid.at(r+1, c).dist_polygon[particle_id];
		return d/(grid.xstep()*grid.ystep());
	}

	double Distance_field_grid_2::gradient_query(const Point_2& p, size_t particle_id, Vector_2& grad) const
	{
		if (p.x() <= grid.xmin() || p.y() <= grid.ymin() || p.x() >= grid.xmax() || p.y() >= grid.ymax() )
		{
			double d = local_query(local_frames[particle_id].local_coord(p), particle_id, grad);
			grad = local_frames[particle_id].world_coord(grad);
			//closest_point = local_frames[polygon_id].world_coord(closest_point);
			return d;
		}
		size_t c = std::floor((p.x() - grid.xmin())/grid.xstep());
		size_t r = std::floor((p.y() - grid.ymin())/grid.ystep());
		double frac_c = p.x() - (grid.xmin() + c*grid.xstep());
		double frac_r = p.y() - (grid.ymin() + r*grid.ystep());
		double cell_area = grid.xstep()*grid.ystep();
		double w[4] = {frac_c*frac_r, (grid.xstep()-frac_c)*(grid.ystep()-frac_r), frac_c*(grid.ystep()-frac_r), (grid.xstep()-frac_c)*frac_r};

		double d = w[0]*grid.at(r+1, c+1).dist_polygon[particle_id] + w[1]*grid.at(r, c).dist_polygon[particle_id];
		d += w[2]*grid.at(r, c+1).dist_polygon[particle_id] + w[3]*grid.at(r+1, c).dist_polygon[particle_id];
		d /= cell_area;

		grad = w[0]*grid.at(r+1, c+1).gradients[particle_id] + w[1]*grid.at(r, c).gradients[particle_id];
		grad = grad + w[2]*grid.at(r, c+1).gradients[particle_id]+w[3]*grid.at(r+1, c).gradients[particle_id];
		grad = grad / cell_area;

		return d;
	}

	void Distance_field_grid_2::update_distance_field(size_t particle_id, const Transformation_2& transformation)
	{
		local_frames[particle_id].transform(transformation);

		// update the field energy at each grid node after the polygon transformed, in an incremental way
		// from left bottom corner (r = 0, c = 0)
		// coordinate in the new local frame
		Vector_2 t_xstep(grid.xstep()*local_frames[particle_id].u.x(), grid.xstep()*local_frames[particle_id].v.x());
		Vector_2 t_ystep(grid.ystep()*local_frames[particle_id].u.y(), grid.ystep()*local_frames[particle_id].v.y());
		Point_2 y_base_pnt = local_frames[particle_id].local_coord(Point_2(grid.at(0, 0).x, grid.at(0, 0).y));

#ifdef _CILK_
		cilk_for (size_t r = 0; r <= grid.nb_rows(); r++)
		{
			Point_2 x_base_pnt = y_base_pnt + r*t_ystep;
			for (size_t c = 0; c <= grid.nb_columns(); c++)
			{
				Vector_2 v;
				//Point_2 closest_pnt;
				grid.at(r, c).dist_polygon[particle_id] = local_query(x_base_pnt, particle_id, v);
				grid.at(r, c).gradients[particle_id] = local_frames[particle_id].world_coord(v);
				//assert(!Geex::Numeric::is_nan(grid.at(r, c).gradients[particle_id].x()));
				//assert(!Geex::Numeric::is_nan(grid.at(r, c).gradients[particle_id].y()));
				//grid.at(r, c).closest_pnts[particle_id] = local_frames[particle_id].world_coord(closest_pnt);
				x_base_pnt = x_base_pnt + t_xstep;				
			}
		}
#else
		for (size_t r = 0; r <= grid.nb_rows(); r++)
		{
			Point_2 x_base_pnt = y_base_pnt;
			for (size_t c = 0; c <= grid.nb_columns(); c++)
			{
				Vector_2 v;
				Point_2 closest_pnt;
				grid.at(r, c).dist_polygon[particle_id] = local_query(x_base_pnt, particle_id, v, closest_pnt);
				grid.at(r, c).gradients[particle_id] = local_frames[particle_id].world_coord(v);
				//grid.at(r, c).closest_pnts[particle_id] = local_frames[particle_id].world_coord(closest_pnt);
				x_base_pnt = x_base_pnt + t_xstep;
			}
			y_base_pnt = y_base_pnt + t_ystep;
		}
#endif
	}

	Discrete_circle_system::Discrete_circle_system(const std::vector<Particle_circle>& particles, const Container_box& bndbox)
		: Circle_system(particles, bndbox), dfg(new Distance_field_grid_2(bndbox, 100, 100))
	{
		for (size_t i = 0; i < particles.size(); i++)
			dfg->append_distance_field(particles[i]);
		sample_pnts.resize(particles.size());

		rm = 0.0;
		for (size_t i = 0; i < particles.size(); i++)
		{
			rm += particles[i].radius();
		}
		const unsigned int sample_nb = 20;
		rm *= boost::math::constants::two_pi<double>();
		rm /= particles.size();
		rm /= 4*sample_nb;
		
		for (size_t i = 0; i < particles.size(); i++)
		{
			sample_pnts[i].reserve(sample_nb);
			double ang_step = boost::math::constants::two_pi<double>()/sample_nb;
			double angle = 0.0;
			while (angle < boost::math::constants::two_pi<double>())
			{
				Point_2 p = particles[i].center() + particles[i].radius()*Vector_2(std::cos(angle), std::sin(angle));
				sample_pnts[i].push_back(p);
				angle += ang_step;
			}
		}

		prev_trans.reserve(particles.size());
		for (size_t i = 0; i < particles.size(); i++)
		{
			prev_trans.push_back(matrix<double>(3,3));
			matrix<double>& m = prev_trans.back();
			m(0,0) = m(1,1) = m(2,2) = 1.0;
			m(0,1) = m(1,0) = m(0,2) = m(2,0) = m(1,2) = m(2,1) = 0.0;
		}
#if KNITRO

#if WITH_ANGLE
		int nb_constraints = particles.size()*2;
		ktr_context = std::auto_ptr<Knitro_context>(new Knitro_context(particles.size()*3, nb_constraints, nb_constraints*2, 0));
		ktr_context->objType = KTR_OBJTYPE_GENERAL;
		ktr_context->objGoal = KTR_OBJGOAL_MINIMIZE;
		for (size_t i = 0; i < particles.size(); i++)
		{
			//ktr_context->xLoBnds[3*i] = -KTR_INFBOUND;
			//ktr_context->xUpBnds[3*i] = KTR_INFBOUND;
			//ktr_context->xLoBnds[3*i+1] = ktr_context->xLoBnds[3*i+2] = -KTR_INFBOUND;
			//ktr_context->xUpBnds[3*i+1] = ktr_context->xUpBnds[3*i+2] = KTR_INFBOUND;
			ktr_context->xLoBnds[3*i] = -3.14;
			ktr_context->xUpBnds[3*i] = 3.14;
			ktr_context->xLoBnds[3*i+1] = bounding_box.x_min() - bounding_box.x_max();
			ktr_context->xLoBnds[3*i+2] = bounding_box.y_min() - bounding_box.y_max();
			ktr_context->xUpBnds[3*i+1] = bounding_box.x_max() - bounding_box.x_min();
			ktr_context->xUpBnds[3*i+2] = bounding_box.y_max() - bounding_box.y_min();
		}

		int *ptrCType = ktr_context->cType;
		double *ptrCLoBnds = ktr_context->cLoBnds, *ptrCUpBnds = ktr_context->cUpBnds;
		for (size_t i = 0; i < particles.size(); i++)
		{
			*ptrCType++ = KTR_CONTYPE_GENERAL;
			*ptrCLoBnds++ = bounding_box.x_min() + particles[i].radius();
			*ptrCUpBnds++ = bounding_box.x_max() - particles[i].radius();
			*ptrCType++ = KTR_CONTYPE_GENERAL;
			*ptrCLoBnds++ = bounding_box.y_min() + particles[i].radius();
			*ptrCUpBnds++ = bounding_box.y_max() - particles[i].radius();
		}
		int constraint_no = 0;
		int *ptrCons = ktr_context->jacIndexCons, *ptrVars = ktr_context->jacIndexVars;
		for (size_t i = 0; i < particles.size(); i++)
		{
			*ptrCons++ = constraint_no;
			*ptrVars++ = 3*i;
			*ptrCons++ = constraint_no;
			*ptrVars++ = 3*i+1;
			//*ptrCons++ = constraint_no;
			//*ptrVars++ = 3*i+2;
			constraint_no++;
			*ptrCons++ = constraint_no;
			*ptrVars++ = 3*i;
			//*ptrCons++ = constraint_no;
			//*ptrVars++ = 3*i+1;
			*ptrCons++ = constraint_no;
			*ptrVars++ = 3*i+2;
			constraint_no++;
		}
#else
		int nb_constraints = particles.size()*2 + particles.size();
		ktr_context = std::auto_ptr<Knitro_context>(new Knitro_context(particles.size()*4, nb_constraints, particles.size()*6+particles.size()*2, 0));
		ktr_context->objType = KTR_OBJTYPE_GENERAL;
		ktr_context->objGoal = KTR_OBJGOAL_MINIMIZE;
		for (size_t i = 0; i < particles.size(); i++)
		{
			ktr_context->xLoBnds[4*i] = -KTR_INFBOUND;
			ktr_context->xUpBnds[4*i] = KTR_INFBOUND;
			ktr_context->xLoBnds[4*i+1] = -KTR_INFBOUND;
			ktr_context->xUpBnds[4*i+1] = KTR_INFBOUND;
			ktr_context->xLoBnds[4*i+2] = -KTR_INFBOUND;
			ktr_context->xUpBnds[4*i+2] = KTR_INFBOUND;
			ktr_context->xLoBnds[4*i+3] = -KTR_INFBOUND;
			ktr_context->xUpBnds[4*i+3] = KTR_INFBOUND;
		}

		int *ptrCType = ktr_context->cType;
		double *ptrCLoBnds = ktr_context->cLoBnds, *ptrCUpBnds = ktr_context->cUpBnds;
		for (size_t i = 0; i < particles.size(); i++)
		{
			*ptrCType++ = KTR_CONTYPE_LINEAR;
			*ptrCLoBnds++ = bounding_box.x_min() + particles[i].radius();
			*ptrCUpBnds++ = bounding_box.x_max() - particles[i].radius();
			*ptrCType++ = KTR_CONTYPE_LINEAR;
			*ptrCLoBnds++ = bounding_box.y_min() + particles[i].radius();
			*ptrCUpBnds++ = bounding_box.y_max() - particles[i].radius();
		}
		for (size_t i = 0; i < particles.size(); i++)
		{
			*ptrCType++ = KTR_CONTYPE_QUADRATIC;
			*ptrCLoBnds++ = 1.0;
			*ptrCUpBnds++ = 1.0;
		}
		int constraint_no = 0;
		int *ptrCons = ktr_context->jacIndexCons, *ptrVars = ktr_context->jacIndexVars;
		for (size_t i = 0; i < particles.size(); i++)
		{
			*ptrCons++ = constraint_no;
			*ptrVars++ = 4*i;
			*ptrCons++ = constraint_no;
			*ptrVars++ = 4*i+1;
			*ptrCons++ = constraint_no;
			*ptrVars++ = 4*i+2;
			constraint_no++;
			*ptrCons++ = constraint_no;
			*ptrVars++ = 4*i;
			*ptrCons++ = constraint_no;
			*ptrVars++ = 4*i+1;
			*ptrCons++ = constraint_no;
			*ptrVars++ = 4*i+3;
			constraint_no++;
		}
		for (size_t i = 0; i < particles.size(); i++)
		{
			*ptrCons++ = constraint_no;
			*ptrVars++ = 4*i;
			*ptrCons++ = constraint_no;
			*ptrVars++ = 4*i + 1;
			constraint_no++;
		}
		for (size_t i = 0; i < particles.size(); i++)
			ktr_context->xInitials[4*i] = 1.0;
#endif 

#endif
	}

	void Discrete_circle_system::move_to_newpoint(const double *const x)
	{
		for (size_t i = 0; i < particle_sys.size(); i++)
		{
			// update the field
#if WITH_ANGLE
			double s = std::sin(x[3*i]), c = std::cos(x[3*i]);
			double tx = x[3*i+1], ty = x[3*i+2];
#else
			double c = x[4*i], s = x[4*i+1];
			double tx = x[4*i+2], ty = x[4*i+3];
#endif
			matrix<double>& m = prev_trans[i];
			Transformation_2 back(	m(0,0), m(1,0), -(m(0,0)*m(0,2)+m(1,0)*m(1,2)),
									m(0,1), m(1,1), -(m(0,1)*m(0,2)+m(1,1)*m(1,2)));
			Transformation_2 t = Transformation_2(c, -s, tx, s, c, ty)*back;
			dfg->update_distance_field(i, t);
			// update the sampling points
			for (size_t j = 0; j < sample_pnts[i].size(); j++)
				sample_pnts[i][j] = t(sample_pnts[i][j]);
			// record the most recent transformation
			m(0,0) = c;
			m(0,1) = -s;
			m(0,2) = tx;
			m(1,0) = s;
			m(1,1) = c;
			m(1,2) = ty;
		}	
	}

	double Discrete_circle_system::molecular_energy_at(const double *const x)
	{
		double LJ_energy = 0.0;
		move_to_newpoint(x);
		for (size_t i = 0; i < particle_sys.size(); i++) // for every polygon
		{
			for (size_t j = 0; j < particle_sys.size(); j++) // for every sample point on each polygon
			{
				if (i == j) continue;
				for (size_t k = 0; k < sample_pnts[i].size(); k++) // compute potential energy with the other polygons pairwise
				{
					double d = dfg->distance_query(sample_pnts[i][k], j);
					if (d <= 0)	d = rm/100;
					double rm6_over_d6 = std::pow(rm/d, 2/*6*/);
					LJ_energy += rm6_over_d6*(rm6_over_d6 - 2);
				}
			}
		}		
		return LJ_energy;
	}

	void Discrete_circle_system::energy_gradient_at(const double *const x, double *gradient)
	{
		move_to_newpoint(x);
		//std::fill(gradient, gradient + ktr_context->n, 0.0);
#if WITH_ANGLE
		std::fill(gradient, gradient + particle_sys.size()*3, 0.0);
#else
		std::fill(gradient, gradient + particle_sys.size()*4, 0.0);
#endif // WITH_ANGLE
		for (size_t i = 0; i < particle_sys.size(); i++)
		{
			matrix<double>& mi = prev_trans[i];
			Transformation_2 backi(	mi(0,0), mi(1,0), -(mi(0,0)*mi(0,2)+mi(1,0)*mi(1,2)),
									mi(0,1), mi(1,1), -(mi(0,1)*mi(0,2)+mi(1,1)*mi(1,2)));
			std::vector<Point_2> original_pnts(sample_pnts[i].size());
			std::transform(sample_pnts[i].begin(), sample_pnts[i].end(), original_pnts.begin(), backi);
#if WITH_ANGLE
			double si = std::sin(x[3*i]), ci = std::cos(x[3*i]);
#endif
			for (size_t j = 0; j < particle_sys.size(); j++)
			{
				if (i==j) continue;
#if WITH_ANGLE
				double sj = std::sin(x[3*j]), cj = std::cos(x[3*j]);
#endif
				matrix<double>& mj = prev_trans[j];
				Transformation_2 backj(	mj(0,0), mj(1,0), -(mj(0,0)*mj(0,2)+mj(1,0)*mj(1,2)),
										mj(0,1), mj(1,1), -(mj(0,1)*mj(0,2)+mj(1,1)*mj(1,2)));				
				for (size_t k = 0; k < sample_pnts[i].size(); k++)
				{
					Vector_2 g;
					double d = dfg->gradient_query(sample_pnts[i][k], j, g);
					double common_factor;
					if (d <= 0)	d = rm/100;

					double rm6_over_d6 = std::pow(rm/d, 2/*6*/);
					common_factor = -/*6*/2*rm6_over_d6*(rm6_over_d6 - 1);
					common_factor /= d*d;
					//common_factor *= epsilon;

					const Point_2& p = original_pnts[k];
#if WITH_ANGLE
					gradient[3*i] += g*Vector_2(-p.x()*si-p.y()*ci, p.x()*ci-p.y()*si)*common_factor;
					assert(!Geex::Numeric::is_nan(gradient[3*i]));
					gradient[3*i+1] += g.x()*common_factor;
					gradient[3*i+2] += g.y()*common_factor;
					Point_2 centerj = particle_sys[j].center();
					gradient[3*j] += -g*Vector_2(-centerj.x()*sj-centerj.y()*cj, centerj.x()*cj-centerj.y()*sj)*common_factor;
					assert(!Geex::Numeric::is_nan(gradient[3*j]));
					gradient[3*j+1] += -g.x()*common_factor;
					gradient[3*j+2] += -g.y()*common_factor;
#else
					gradient[4*i] += g*Vector_2(p.x(), p.y())*common_factor;
					assert(!Geex::Numeric::is_nan(gradient[4*i]));
					gradient[4*i+1] += g*Vector_2(-p.y(), p.x())*common_factor;
					assert(!Geex::Numeric::is_nan(gradient[4*i+1]));
					gradient[4*i+2] += g.x()*common_factor;
					gradient[4*i+3] += g.y()*common_factor;
					Point_2 centerj = particle_sys[j].center();
					gradient[4*j] += -g*Vector_2(centerj.x(), centerj.y())*common_factor;
					assert(!Geex::Numeric::is_nan(gradient[4*j]));
					gradient[4*j+1] += -g*Vector_2(-centerj.y(), centerj.x())*common_factor;
					gradient[4*j+2] += -g.x()*common_factor;
					gradient[4*j+3] += -g.y()*common_factor;
#endif
				}
			}
		}
	}
	void Discrete_circle_system::constraint_at(const double *const x, double *constraint_values)
	{
		for (size_t i = 0; i < particle_sys.size(); i++)
		{
#if	WITH_ANGLE
			double s = std::sin(x[3*i]), c = std::cos(x[3*i]);
			double tx = x[3*i+1], ty = x[3*i+2];
#else
			double c = x[4*i], s = x[4*i+1];
			double tx = x[4*i+2], ty = x[4*i+3];
#endif
			Transformation_2 t(c, -s, tx, s, c, ty);
			Point_2 p = t(particle_sys[i].center());
			*constraint_values++ = p.x();
			*constraint_values++ = p.y();
		}
#if !WITH_ANGLE
		for (size_t i = 0; i < particle_sys.size(); i++)
			*constraint_values++ = x[4*i]*x[4*i] + x[4*i+1]*x[4*i+1];
#endif

	}
	void Discrete_circle_system::constraint_grad_at(const double *const x, double *constraint_grad)
	{
		for (size_t i = 0; i < particle_sys.size(); i++)
		{
#if KNITRO		//************	Knitro  *************

#if WITH_ANGLE
			double s = std::sin(x[3*i]), c = std::cos(x[3*i]);
#else
			double c = x[4*i], s = x[4*i+1];
#endif
			Point_2 p = particle_sys[i].center();
#if WITH_ANGLE
			*constraint_grad++ = -p.x()*s - p.y()*c;
			*constraint_grad++ = 1.0;
			*constraint_grad++ = p.x()*c-p.y()*s;
 			*constraint_grad++ = 1.0;			
#else
			*constraint_grad++ = p.x();
			*constraint_grad++ = -p.y();
			*constraint_grad++ = 1.0;
			*constraint_grad++ = p.y();
			*constraint_grad++ = p.x();
			*constraint_grad++ = 1.0;
#endif
#else
			//***********	Worhp   *************
			double s = std::sin(x[3*i]), c = std::cos(x[3*i]);
			Point_2 p = particle_sys[i].center();
			*constraint_grad++ = -p.x()*s - p.y()*c;
			*constraint_grad++ = p.x()*c-p.y()*s;
			*constraint_grad++ = 1.0;
			//*constraint_grad++ = 0.0;
			//*constraint_grad++ = 0.0;
			*constraint_grad++ = 1.0;
#endif
		}
#if !WITH_ANGLE
		for (size_t i = 0; i < particle_sys.size(); i++)
		{
			*constraint_grad++ = 2*x[4*i];
			*constraint_grad++ = 2*x[4*i+1];
		}
#endif
	}
	int Discrete_circle_system::optimize(double *trans, bool apply_constraints)
	{
#if KNITRO
		// ********************************	Knitro**************************************
		KTR_context* kc = ktr_context->kc;
		if (kc == NULL)
			return -1000 ; // probably a license issue
		if (KTR_set_int_param_by_name(kc, "algorithm", 1) != 0)
			return -1000;
		/* set options: automatic gradient and hessian matrix */
		if (KTR_set_int_param_by_name(kc, "bar_murule", 3) != 0) // "0" and "3" is plausible up to now if the above algo para is "1". "6" a bit less plausible
			return -1000;
		if (KTR_set_int_param_by_name(kc, "gradopt", KTR_GRADOPT_EXACT) != 0)
			return -1000 ;
		if (KTR_set_int_param_by_name(kc, "hessopt", KTR_HESSOPT_BFGS) != 0)
			return -1000 ;
		if (KTR_set_int_param_by_name(kc, "bar_directinterval", 5) != 0)
			return -1000;
		//if (KTR_set_int_param_by_name(kc, "maxcgit", 1) != 0)
		//	return -1000;
		// debug parameters
		if (KTR_set_int_param_by_name(kc, "debug", 0) != 0)
			return -1000;
		if (KTR_set_int_param_by_name(kc, "outlev", 3) != 0)
			return -1000 ;
		if (KTR_set_int_param_by_name(kc, "outmode", 0) != 0)
			return -1000;
		//if (KTR_set_int_param_by_name(kc, "newpoint", 3) != 0)
		//	return -1000;

		if (KTR_set_int_param_by_name(kc, "maxit", 0) != 0)
			return -1000 ;
		if (KTR_set_double_param_by_name(kc, "opttol", 1e-9) != 0)
			return -1000;
		if (KTR_set_double_param_by_name(kc, "xtol", 1e-15) != 0)
			return -1000;
		if (KTR_set_int_param_by_name(kc, "bar_maxbacktrack", 10) != 0)
			return -1000;
		//if (KTR_set_int_param_by_name(kc, "ms_enable", 1) != 0)
		//	return -1000;
		/* register the callback function */
		if (KTR_set_func_callback(kc, &callback) != 0)
			return -1000 ;
		if (KTR_set_grad_callback(kc, &callback) != 0)
			return -1000 ;
		//if (KTR_set_newpoint_callback(kc, &newpoint_callback) != 0)
		//	return -1000;

		int nStatus = KTR_init_problem(	kc, ktr_context->n, ktr_context->objGoal, ktr_context->objType, 
										ktr_context->xLoBnds, ktr_context->xUpBnds, 
										ktr_context->m, ktr_context->cType, ktr_context->cLoBnds, ktr_context->cUpBnds,
										ktr_context->nnzJ, ktr_context->jacIndexVars, ktr_context->jacIndexCons,
										ktr_context->nnzH, ktr_context->hessIndexRows, ktr_context->hessIndexCols, ktr_context->xInitials, NULL);
		if (nStatus)	return nStatus;
		nStatus = KTR_solve(kc, trans, ktr_context->lambda, 0, &ktr_context->obj, NULL, NULL, NULL, NULL, NULL, this);
		return nStatus;
#else

	//	********************* WORHP	**********************
		int status = OK;
		OptVar prb_desc; // problem descriptor
		Workspace wksp;
		Params para;
		Control ctrl;
		prb_desc.initialised = false;
		wksp.initialised = false;
		para.initialised = false;
		ctrl.initialised = false;

		para.MatrixCC = false;
		prb_desc.n = particle_sys.size()*3;
		prb_desc.m = particle_sys.size()*2;

		wksp.DF.nnz = prb_desc.n;
		wksp.DG.nnz = prb_desc.m*2;
		//wksp.DG.nnz = 0;
		wksp.HM.nnz = WorhpMatrix_Init_Dense;
		WorhpInit(&prb_desc, &wksp, &para, &ctrl);
		WorhpSetBoolParam(&para, "UserHM", false);
		WorhpSetBoolParam(&para, "FidifHM", false);
		//assert(WorhpSetBoolParam(&para, "UserHM", false));
		//assert(WorhpSetBoolParam(&para, "FidifHM", false));		
		if (ctrl.status != FirstCall)
		{
			std::cout<< "Initialization failed.\n";
			return -1000;
		}
		for (int i = 0; i < prb_desc.n; i++)
		{
			prb_desc.X[i] = 0.0;
			prb_desc.Lambda[i] = 0.0;
			prb_desc.XL[i] = -para.Infty;
			prb_desc.XU[i] = para.Infty;
			//if (i%3 == 0)
			//{
			//	prb_desc.XL[i] = -3.14;
			//	prb_desc.XU[i] = 3.14;
			//}
			//else if (i%3 == 1)
			//{
			//	prb_desc.XL[i] = bounding_box.x_min() - bounding_box.x_max();
			//	prb_desc.XU[i] = bounding_box.x_max() - bounding_box.x_min();
			//}
			//else
			//{
			//	prb_desc.XL[i] = bounding_box.y_min() - bounding_box.y_max();
			//	prb_desc.XU[i] = bounding_box.y_max() - bounding_box.y_min();
			//}
		}
		for (int i = 0; i < particle_sys.size(); i++)
		{
			prb_desc.Mu[2*i] = prb_desc.Mu[2*i+1] = 0.0;
			prb_desc.GL[2*i] = bounding_box.x_min() + particle_sys[i].radius();
			prb_desc.GU[2*i] = bounding_box.x_max() - particle_sys[i].radius();
			prb_desc.GL[2*i+1] = bounding_box.y_min() + particle_sys[i].radius();
			prb_desc.GU[2*i+1] = bounding_box.y_max() - particle_sys[i].radius();
		}
		if (wksp.DG.NeedStructure)
		{
			//int constraint_no = 0;
			int *prow = wksp.DG.row, *pcol = wksp.DG.col;
			for (int i = 0; i < particle_sys.size(); i++)
			{
				// theta
				*pcol++ = 3*i+1;
				*prow++ = 2*i+1;
				*pcol++ = 3*i+1;
				*prow++ = 2*i+1+1;
				// x
				*pcol++ = 3*i+1+1;
				*prow++ = 2*i+1;
				//*pcol++ = 3*i+1+1;
				//*prow++ = 2*i+1+1;
				//*pcol++ = 3*i+2+1;
				//*prow++ = 2*i+1;
				// y
				*pcol++ = 3*i+2+1;
				*prow++ = 2*i+1+1;
			}
		}
		assert(!wksp.DF.NeedStructure);
		assert(!wksp.HM.NeedStructure);
		while (ctrl.status < TerminateSuccess && ctrl.status > TerminateError)
		{
			if (GetUserAction(&ctrl, callWorhp))
			{
				Worhp(&prb_desc, &wksp, &para, &ctrl);
			}
			if (GetUserAction(&ctrl, evalF))
			{
				prb_desc.F = molecular_energy_at(prb_desc.X);
				//std::cout<<prb_desc.F<<std::endl;
				DoneUserAction(&ctrl, evalF);
			}
			if (GetUserAction(&ctrl, evalG))
			{
				constraint_at(prb_desc.X, prb_desc.G);
				DoneUserAction(&ctrl, evalG);
			}
			if (GetUserAction(&ctrl, evalDF))
			{
				energy_gradient_at(prb_desc.X, wksp.DF.val);
				DoneUserAction(&ctrl, evalDF);
			}
			if (GetUserAction(&ctrl, evalDG))
			{
				constraint_grad_at(prb_desc.X, wksp.DG.val);
				DoneUserAction(&ctrl, evalDG);
			}
			if (GetUserAction(&ctrl, iterOutput))
			{
				// debug info here
				//IterationOutput(&prb_desc, &wksp, &para, &ctrl);
				//std::cout<<wksp.ScaledKKT<<std::endl;
				DoneUserAction(&ctrl, iterOutput);
			}
			assert(!GetUserAction(&ctrl, evalHM));
		}

		std::copy(prb_desc.X, prb_desc.X+prb_desc.n, trans);
		StatusMsg(&prb_desc, &wksp, &para, &ctrl);
		WorhpFree(&prb_desc, &wksp, &para, &ctrl);
		//std::cout<<"Status = "<< ctrl.status<<std::endl;
		return ctrl.status;
#endif
	}
}